#include "mpu6050.h"
#include "peripherals_i2c_0_config.h"
#include "board_delay.h"
#include <math.h>
#include <stdio.h>

#define MPU6050_I2C_ADDR         (0x68)
#define MPU6050_REG_PWR_MGMT_1   (0x6B)
#define MPU6050_REG_ACCEL_XOUT_H (0x3B)
#define MPU6050_DATA_LEN         (14)
#define OSIF_WAIT_FOREVER        (0xFFFFFFFFU)

static int MPU6050_SetI2CAddr(void)
{
    // 适配I2C接口
    I2C_DRV_MasterSetSlaveAddr(INST_I2C_0, MPU6050_I2C_ADDR, false);
    return 0;
}

int MPU6050_Init(void)
{
    uint8_t buf[2];
    buf[0] = MPU6050_REG_PWR_MGMT_1;
    buf[1] = 0x00; // 唤醒
    MPU6050_SetI2CAddr();
    if (I2C_DRV_MasterSendDataBlocking(INST_I2C_0, buf, 2, true, OSIF_WAIT_FOREVER) != STATUS_SUCCESS) {
        return -1;
    }
    simple_delay_ms(10);
    return 0;
}

int MPU6050_ReadRawData(MPU6050_RawData_t* data)
{
    if (!data) return -1;
    uint8_t reg = MPU6050_REG_ACCEL_XOUT_H;
    uint8_t buf[MPU6050_DATA_LEN];
    MPU6050_SetI2CAddr();
    if (I2C_DRV_MasterSendDataBlocking(INST_I2C_0, &reg, 1, false, OSIF_WAIT_FOREVER) != STATUS_SUCCESS) {
        return -1;
    }
    if (I2C_DRV_MasterReceiveDataBlocking(INST_I2C_0, buf, MPU6050_DATA_LEN, true, OSIF_WAIT_FOREVER) != STATUS_SUCCESS) {
        return -2;
    }
    data->ax = (int16_t)((buf[0] << 8) | buf[1]);
    data->ay = (int16_t)((buf[2] << 8) | buf[3]);
    data->az = (int16_t)((buf[4] << 8) | buf[5]);
    data->temp = (int16_t)((buf[6] << 8) | buf[7]);
    data->gx = (int16_t)((buf[8] << 8) | buf[9]);
    data->gy = (int16_t)((buf[10] << 8) | buf[11]);
    data->gz = (int16_t)((buf[12] << 8) | buf[13]);
    return 0;
}

void MPU6050_CalcAttitude(const MPU6050_RawData_t* raw, MPU6050_Attitude_t* att)
{
    if (!raw || !att) return;
    // 简单互补滤波/欧拉角计算（仅基于加速度，适合静止或低速）
    float ax = raw->ax / 16384.0f;
    float ay = raw->ay / 16384.0f;
    float az = raw->az / 16384.0f;
    att->pitch = atan2f(ax, sqrtf(ay * ay + az * az)) * 57.2958f; // 180/PI
    att->roll  = atan2f(ay, sqrtf(ax * ax + az * az)) * 57.2958f;
    att->yaw   = 0.0f; // 无磁力计，无法直接算yaw
}

int MPU6050_GetAttitude(MPU6050_Attitude_t* att)
{
    MPU6050_RawData_t raw;
    if (MPU6050_ReadRawData(&raw) != 0) return -1;
    MPU6050_CalcAttitude(&raw, att);
    return 0;
} 